﻿#include "M2P_Serial_Console.h"
#include "M2M_Serial_Console.h"
#include "USART1_DMA.h"
#include "USART2_DMA.h"
#include "speed_reading.h"
#include "PWM.h"
#include "NumTOStr.h"
#include "vofa+.h"

u8 Flag_M2PSend=0;
u8 M2PSerial_RxCmd[M2PSerial_CmdUnit+1]="";//接收容器
u8 M2PSerial_TxCmd[M2PSerial_CmdUnit+1]="";//发送容器
u8 M2PSerial_TxMode=1;
u8 M2PSerial_AutoSend=1;
u16 M2PSerial_TxRoutine_10ms=10;
char InvaildSet[]="Invaild Setting Cmd!\r\n";
char InvaildCmd1[]="Invaild Control Cmd1!\r\n";
char InvaildCmd2[]="Invaild Control Cmd2!\r\n";
void M2PSerial_Control(void)
{
    if(Flag_M2PSend)
    {
        
        M2PSerial_CmdToTx();
        Flag_M2PSend=0;
    }
    if(USART1_RX_FLAG)
    {
        M2PSerial_RxToCmd();
        USART1_RX_FLAG=0;
    }
}
void M2PSerial_RxToCmd(void)  //将接收到的串口数据转换为程控指令
{
    int i,M2PSerial_Test=0;
    
    //上位机测试信号判定
    for(i=0;i<M2PSerial_CmdUnit;i++)
    {
        if(M2PSerial_RxCmd[i]==OK_Respond[i])
        {
            M2PSerial_Test=1;
        }
        else
        {
            M2PSerial_Test=0;
            break;
        }
    }

    if(M2PSerial_Test)//是测试包
    {
        m2p_printf("JHY M2P Test OK!\r\n");
        return;
    }
    else if(M2PSerial_RxCmd[0]=='J'&&M2PSerial_RxCmd[10]=='\r'&&M2PSerial_RxCmd[11]=='\n')//不是普通的引导指令，是对副控制板相关参数的修改指令
    {//Switch大型嵌套预警！
        switch (M2PSerial_RxCmd[1])//控制位检查
        {
        case 0x01://控制位信息：读取参数修改
            switch (M2PSerial_RxCmd[2])//标志位1检查
            {
            case 0x01://发送模式更改
                if(M2PSerial_RxCmd[3]<=4)
                    M2PSerial_TxMode=M2PSerial_RxCmd[3];
                else
                    m2p_printf("Invaild Txmode\r\n");//没有对应的修改指令
                break;
            case 0x02://发送延时更改
                if(M2PSerial_RxCmd[3]>=4)
                    M2PSerial_TxRoutine_10ms=M2PSerial_RxCmd[3];
                else
                    m2p_printf("Invaild TxSendTime\r\n");//没有对应的修改指令
                break;
            case 0x03://PID参数查询
                for(i=0;i<4;i++)
                {
                    m2p_printf("motor%d PID:KP%4f,KI%4f,KD%4f\r\n",i,Kp[i],Ki[i],Kd[i]);
                }
                break;
            case 0x04://速度参数查询
                for(i=0;i<MainMotor_SpeedLevel;i++)
                {
                    m2p_printf("speed[%d]=%d\r\n",i,MainMotor_Speed[i]);
                }
                break;
            default:
                m2p_printf(InvaildCmd1);//没有对应的修改指令
                break;
            }
            break;
        case 0x02://控制位信息：主板参数修改
            switch (M2PSerial_RxCmd[2])//标志位1检查
            {
            case 0x01://PID参数控制
               if(!M2PSerial_RxCmd[3])//标志位2检查是0，则PID参数统一更改
               {
                   for(int i=0;i<4;i++)
                   {
                        if(StrtoNum_4digit(M2PSerial_RxCmd,4)!=0)      Kp[i]=StrtoNum_4digit(M2PSerial_RxCmd,4)/10000.0;
                        if(StrtoNum_4digit(M2PSerial_RxCmd,6)!=0)      Ki[i]=StrtoNum_4digit(M2PSerial_RxCmd,6)/10000.0;
                        if(StrtoNum_4digit(M2PSerial_RxCmd,8)!=0)      Kd[i]=StrtoNum_4digit(M2PSerial_RxCmd,8)/10000.0;
                   }
               }
               else if(M2PSerial_RxCmd[3]<=4)//1-4对应单台电机0-3的PID参数
               {
                    if(StrtoNum_4digit(M2PSerial_RxCmd,4)!=0)      Kp[M2PSerial_RxCmd[3]-1]=StrtoNum_4digit(M2PSerial_RxCmd,4)/10000.0;
                    if(StrtoNum_4digit(M2PSerial_RxCmd,6)!=0)      Ki[M2PSerial_RxCmd[3]-1]=StrtoNum_4digit(M2PSerial_RxCmd,6)/10000.0;
                    if(StrtoNum_4digit(M2PSerial_RxCmd,8)!=0)      Kd[M2PSerial_RxCmd[3]-1]=StrtoNum_4digit(M2PSerial_RxCmd,8)/10000.0;
               }
               else
               {
                   m2p_printf(InvaildCmd2);
               }
               break;
            case 0x02://主底盘电机各档位速度调节
                switch (M2PSerial_RxCmd[3])
                {
                case 0x01:
                    if((u16)StrtoNum_4digit(M2PSerial_RxCmd,4)!=0)   MainMotor_Speed[1]=(u16)StrtoNum_4digit(M2PSerial_RxCmd,4);
                    if((u16)StrtoNum_4digit(M2PSerial_RxCmd,6)!=0)   MainMotor_Speed[2]=(u16)StrtoNum_4digit(M2PSerial_RxCmd,6);
                    break;
                case 0x02:
                    if((u16)StrtoNum_4digit(M2PSerial_RxCmd,4)!=0)   MainMotor_Speed[3]=(u16)StrtoNum_4digit(M2PSerial_RxCmd,4);
                    if((u16)StrtoNum_4digit(M2PSerial_RxCmd,6)!=0)   MainMotor_Speed[4]=(u16)StrtoNum_4digit(M2PSerial_RxCmd,6);
                    break;
                default:
                    m2p_printf(InvaildCmd2);//没有对应的修改指令
                    break;
                }
                break;
            default:
                m2p_printf(InvaildCmd1);//没有对应的修改指令
                break;
            }
            break;
        case 0x03://控制位信息：副板参数修改，将参数不做修改地转发给副板！
            DMA_USART2_Tx_Data(M2PSerial_RxCmd,M2MSerial_CmdUnit);
            break;
        default:
            m2p_printf(InvaildSet);
            break;
        }
        
    }
    else//帧头帧尾校验出错
    {
        m2p_printf("Invaild protocol data!\r\n");
        return;
    }
    
}
void M2PSerial_CmdToTx(void) //将程控指令转换为串口数据发送出去
{
    float VOFA_Data[20]={0};
    switch (M2PSerial_TxMode)
    {
    case 0x00:
        break;
    case 0x01:
        m2p_printf("current motor speed:%d,%d,%d,%d\r\n",Current_Inpluse[0],Current_Inpluse[1],Current_Inpluse[2],Current_Inpluse[3]);
        break;
    case 0x02:
        m2p_printf("current PWM output:%d,%d,%d,%d\r\n",PWM1_value[0],PWM1_value[1],PWM1_value[2],PWM1_value[3]);
        break;
    case 0x03:
        m2p_printf("TargetSpeed:%d,%d,%d,%d,direction:%d\r\n",Target_Inpluse[0],Target_Inpluse[1],Target_Inpluse[2],Target_Inpluse[3],M2MSerial_RxCmd[2]);
        break;
    /*
    case 0x03://VOFA模式
        for(i=0;i<4;i++)
        {
            VOFA_Data[i]=Current_Inpluse[i];
        }
        JustFloat_M2PSend(VOFA_Data,4);
        break;
    case 0x04:
        for(i=0;i<4;i++)
        {
            VOFA_Data[i]=PWM1_value[i];
        }
        JustFloat_M2PSend(VOFA_Data,4);
        break;
    case 0x05:
        for(i=0;i<4;i++)
        {
            VOFA_Data[i]=Target_Inpluse[1];
        }
        JustFloat_M2PSend(VOFA_Data,4);
        break;
    */
    default:
        break;
    }
}

void M2PSerial_ConnectionStatueCheck(void)		//检查连接状态，报心跳接收超时和发送无反应出错
{

}
void M2PSerial_ErrorReaction(void)   //判断串口数据的错误反馈并作出反应
{

}
